#ifndef SPORTS_CONTROLLER_H
#define SPORTS_CONTROLLER_H

#include "srpq_controller/FSM_States/FSM_State.h"
#include "common/Controllers/FootSwingTrajectory.h"

struct Sports_Jump {
    bool jump_pending = false;
    bool jump_in_progress = false;
    bool pressed = false;
    int START_ITER = 0;
    int END_ITER = 0;
    int PRO_COUNT = 1000;

    bool trigger_pressed(int iter, bool trigger) {
        if (!pressed && trigger) {
            if (!jump_pending && !jump_in_progress) {
                jump_pending = true;
                START_ITER = iter;
                END_ITER = START_ITER + PRO_COUNT;
                return true;
            }
            return false;
        }
        pressed = trigger;
        return false;
    }

    bool should_jump(int iter) {
        if (jump_pending && iter >= START_ITER && iter <= END_ITER) {
            jump_pending = false;
            jump_in_progress = true;
            return true;
        }

        if (jump_in_progress) {
            if (iter >= END_ITER) {
                jump_in_progress = false;
                return false;
            }
            return true;
        }
        return false;
    }
};

/**
 *
 */
/*BLANCED*/
template <typename T>
struct DogModel {
    Vec3<T> OQ_init;
    Vec3<T> OP_init;
    Vec3<T> QP_O;
    Vec3<T> QP_Q;
    Vec3<T> OQ_now;
};

template <typename T>
class SportsController {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    SportsController(ControlFSMData<T>* _controlFSMData);
    void run(void);

private:
    void Stand(void);
    void Trot(T upAmp, T downAmp);
    void Jump(void);
    void Front_Jump_Low(void);
    void Front_Jump_High(void);
    void Walk(T upAmp, T downAmp);
    ControlFSMData<T>* _data;
    Sports_Jump jump_state;
    // Keep track of the control iterations
    int iter = 0;
    int currentSeq = 0;
    bool currently_jumping = false;

    FootSwingTrajectory<T> _footSwingTrajectory;
    T _ini_height = -0.28; 
    T _ini_xbias=0.05;
    T _ini_yaw = 0.0;

    // jump state
    Vec3<T> front_jump_begin;
    Vec3<T> front_jump_end;

    // temp
    DogModel<T> _dogModel[4];
    T length = 0.522;
    T width = 0.290;
    void calculatePosBias_ang(T roll, T pitch, T yaw);
    void calculatePosBias_rad(T roll, T pitch, T yaw);
    Vec3<T> calculateProgressPos(const int& curr_count, int start_count, int finish_count, const Vec3<T>& ini, const Vec3<T>& fin);
    void calculateJumpPos(T pitch, T height, T length,Vec3<T> ref, Vec3<T>*begin, Vec3<T>*end);
    void limitRpy(Vec3<T>& rpy, T max, T threshold, T alpha);
    void _SetupCommand(void);
    Vec3<T> limit(Vec3<T> in, Vec3<T>min, Vec3<T> max);

};

#endif    // FSM_STATE_LOCOMOTION_H
